
#include <stddef.h>
#include <string.h>
#include "MessageBus.h"
#include "MessageDefine.h"

#include "mbed_critical.h"

#define MsgMem_EnterCriticel()		core_util_critical_section_enter()
#define MsgMem_ExitCriticel()		core_util_critical_section_exit()

#define MsgMem_SIZE		256

typedef unsigned int MsgMem_T;

static MsgMem_T MsgMemBase[MsgMem_SIZE];
static MsgMem_T MsgMemAvail;  //1st index of the 1st free range

#define GetSize(x)	MsgMemBase[x]
#define GetLink(x)	MsgMemBase[(x)+1]
#define SetSize(x, s)	do{MsgMemBase[x] = (s);}while(0)
#define SetLink(x, v)	do{MsgMemBase[(x)+1] = (v);}while(0)

static void* MsgMem_DoAlloc(unsigned n) {
	MsgMem_T current, previous, remain;

	n++; //增加保存分配块大小的空间
	for (current = previous = MsgMemAvail; current != MsgMem_SIZE; current = GetLink(current)) { //查找连续空间满足的块
		if (GetSize(current) >= n) {
			break;
		}
		previous = current;
	}

	if (current == MsgMem_SIZE) {//没有找到满足条件的块
		return NULL;
	}

	remain = GetSize(current) - n;
	if (remain < 2) { //没有足够的空间存储Link和Size，整块分配
		if (current == MsgMemAvail) { //首块即匹配
			MsgMemAvail = GetLink(current);
		}
		else {// 删除分配的块
			SetLink(previous, GetLink(current));
		}
		remain = 0;
	}
	else {
		SetSize(current, remain); // 设置被分配块的大小为剩余的大小
		SetSize(current + remain, n); //从块的末尾分配空间
	}
	return (void*)&MsgMemBase[current + remain + 1]; //返回Size后面的地址
}

static void MsgMem_DoFree(MsgMem_T* f, unsigned n) {
	MsgMem_T current, previous, toFree;

	toFree = f - MsgMemBase;
	if (toFree < MsgMemAvail) { //释放的块在avail之前
		if (((toFree + n) == MsgMemAvail) && MsgMemAvail < MsgMem_SIZE) { //被释放的块与其后的自由块连续
			SetSize(toFree, n + GetSize(MsgMemAvail)); //合并 toFree与avail
			SetLink(toFree, GetLink(MsgMemAvail));
		}
		else{
			SetLink(toFree, MsgMemAvail); 
		}
		MsgMemAvail = toFree;
	}
	else {
		current = previous = MsgMemAvail;
		while ((toFree > current) && (current != MsgMem_SIZE)){ //查找被释放块的前、后自由块
			previous = current;
			current = GetLink(previous);
		}

		if (((toFree + n) == current) && current != MsgMem_SIZE) {//toFree与其后的自由块连续
			n += GetSize(current);	//合并toFree 与其后的自由块
			SetLink(toFree, GetLink(current));
		}
		else{
			SetLink(toFree, current);
		}

		if ((previous + GetSize(previous)) == toFree) { //toFree与其前的自由块连续
			SetSize(previous, GetSize(previous) + n); //合并toFree 与 其前的自由块
			SetLink(previous, GetLink(toFree));
		}
		else {
			SetSize(toFree, n);
			SetLink(previous, toFree);
		}
	}
}

void MsgMem_Init(void) {
	SetSize(MsgMemAvail, MsgMem_SIZE);
	SetLink(MsgMemAvail, MsgMem_SIZE);
}

void* MsgMem_Alloc(unsigned n) {
	MsgMem_T num;
	void* p;
	
	if (n == 0) {
		return NULL;
	}
	n += sizeof(MsgMem_T) - 1; //计算对齐大小
	num = n / sizeof(MsgMem_T); 
	MsgMem_EnterCriticel();
	p = MsgMem_DoAlloc(num);
	MsgMem_ExitCriticel();
	return p;
}

void MsgMem_Free(void* f) {
	if (&MsgMemBase[0] < (MsgMem_T*)f && (MsgMem_T*)f < &MsgMemBase[MsgMem_SIZE]) {
		MsgMem_T* p = (MsgMem_T*)f - 1;
		
		MsgMem_EnterCriticel();
		MsgMem_DoFree(p, *p);
		MsgMem_ExitCriticel();
	}
}




